import json
import socket
import threading
from time import sleep
import matplotlib.pyplot as plt
import pro.projection as projection

import numpy as np
# 设置中文字体支持（可选）
plt.rcParams['font.sans-serif'] = ['SimHei']  # 用来正常显示中文标签
plt.rcParams['axes.unicode_minus'] = False  # 用来正常显示负号
plt.ion()  # 开启交互模式
# 计算距离
def distance(p1,p2):
    # 定义两个点的坐标
    point1 = np.array(p1)
    point2 = np.array(p2)

    # 计算欧氏距离
    distance = np.linalg.norm(point2 - point1)
    # print("欧氏距离为:", distance)
    return distance

# 创建图形和坐标轴
fig, ax = plt.subplots(figsize=(8, 6))




# 定义起止点线段
def line(x1,y1,x2,y2):
    return [x1,y1],[x2,y2]

# 接收数据的线程函数
def recv_thread(udp_socket):
    while True:
        recv_data = udp_socket.recvfrom(1024)
        # print("[From %s:%d]:%s" % (recv_data[1][0], recv_data[1][1], recv_data[0].decode("utf-8")))
        pos = json.loads(recv_data[0].decode("utf-8"))
        point = [pos["location"]["x"],pos["location"]["y"]]
        print(point)

        calAndDraw(point)

def start_receive():
    udp_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    # 绑定套接字到地址
    udp_socket.bind(('', 11006))
    # t = threading.Thread(target=recv_thread,args=(udp_socket,))
    # t.start()
    # t.join()
    recv_thread(udp_socket)
    udp_socket.close()


line_1_start,line_1_end = line(27090,30431,40623,30382)
line_2_start,line_2_end = line(40623,30382,40623,1379)
# line_3_start,line_3_end = line(80,60,80,1)
# line_4_start,line_4_end = line(80,60,1,60)

points_org, = ax.plot([], [], 'ro')  # 空点集
points_res, = ax.plot([], [], 'go')  # 空点集
x_data = []
y_data = []

def calAndDraw(point):
    ax.cla()
    drawLine()
    ax.plot(point[0],point[1],'ro')
    p1 = projection.point_to_line_projection(point,line_1_start,line_1_end)
    p2 = projection.point_to_line_projection(point,line_2_start,line_2_end)
    # p3 = projection.point_to_line_projection(point,line_3_start,line_3_end)
    # p4 = projection.point_to_line_projection(point,line_4_start,line_4_end)

    d1 = distance(point,p1)
    d2 = distance(point,p2)
    # d3 = distance(point,p3)
    # d4 = distance(point,p4)

    car = [0,0]

    if d1 < d2 :
        car = p1
    if d2 < d1 :
        car = p2
    # if d3 < d2 and d3 < d1 and d3 < d4:
    #     car = p3
    # if d4 < d2 and d4 < d3 and d4 < d1:
    #     car = p4
    if point is None:
        return
    ax.plot(car[0],car[1],'gX')

    # 重绘图形
    fig.canvas.draw()
    fig.canvas.flush_events()
    # sleep(0.3)

def drawLine():
    ax.set_title('小车实时位置')
    ax.set_xlim(25000, 50000)
    ax.set_ylim(1000, 41000)
    ax.grid(True)
    ax.set_aspect('equal')
    ax.plot([line_1_start[0],line_1_end[0]],[line_1_start[1],line_1_end[1]],color='red',linestyle='-',marker='x')
    ax.plot([line_2_start[0],line_2_end[0]],[line_2_start[1],line_2_end[1]],color='red',linestyle='-',marker='x')
    # ax.plot([line_3_start[0],line_3_end[0]],[line_3_start[1],line_3_end[1]],color='red',linestyle='-',marker='x')
    # ax.plot([line_4_start[0],line_4_end[0]],[line_4_start[1],line_4_end[1]],color='red',linestyle='-',marker='x')

start_receive()
plt.ioff()
plt.show()

